#include "motor.h"


int Target_Speed[4] = {0},
    PWM[4] = {0};
// p 5 d 0.8
uint8_t MOTOR_Dir[4] = {0};
float Kp[4] = {10,10,10,10},
     Ki[4] = {1,1,1,1};


void Motor_init()
{
    HAL_TIM_PWM_Start(&HTIM_MOTOR_FL, CHANNEL_FL_A);
	HAL_TIM_PWM_Start(&HTIM_MOTOR_FL, CHANNEL_FL_B);
	HAL_TIM_PWM_Start(&HTIM_MOTOR_FR, CHANNEL_FR_A);
	HAL_TIM_PWM_Start(&HTIM_MOTOR_FR, CHANNEL_FR_B);
	HAL_TIM_PWM_Start(&HTIM_MOTOR_BL, CHANNEL_BL_A);
	HAL_TIM_PWM_Start(&HTIM_MOTOR_BL, CHANNEL_BL_B);
	HAL_TIM_PWM_Start(&HTIM_MOTOR_BR, CHANNEL_BR_A);
	HAL_TIM_PWM_Start(&HTIM_MOTOR_BR, CHANNEL_BR_B);
    __HAL_TIM_SET_COMPARE(&HTIM_MOTOR_FL, CHANNEL_FL_A, 0 );
    __HAL_TIM_SET_COMPARE(&HTIM_MOTOR_FL, CHANNEL_FL_B, 0 );
    __HAL_TIM_SET_COMPARE(&HTIM_MOTOR_FR, CHANNEL_FR_A, 0 );
    __HAL_TIM_SET_COMPARE(&HTIM_MOTOR_FR, CHANNEL_FR_B, 0 );
    __HAL_TIM_SET_COMPARE(&HTIM_MOTOR_BL, CHANNEL_BL_A, 0 );
    __HAL_TIM_SET_COMPARE(&HTIM_MOTOR_BL, CHANNEL_BL_B, 0 );
    __HAL_TIM_SET_COMPARE(&HTIM_MOTOR_BR, CHANNEL_BR_A, 0 );
    __HAL_TIM_SET_COMPARE(&HTIM_MOTOR_BR, CHANNEL_BR_B, 0 );
    __HAL_TIM_CLEAR_IT(&htim7, TIM_SR_UIF);
    HAL_TIM_Base_Start_IT(&htim7);
}

void Motor_Target_Limit(int16_t* Target_Array)
{
    uint16_t max;
    uint8_t i;
    if(Target_Array[0] > PWM_MAX ||Target_Array[1] > PWM_MAX 
        ||Target_Array[2] > PWM_MAX ||Target_Array[3 > PWM_MAX]){
        max=Target_Array[0];
        for(i=0; i<4; ++i){
            if(Target_Array[i] > max)
                max = Target_Array[i];
        }
        for (i = 0; i < 4; ++i)
        {
            Target_Array[i] = Target_Array[i] * (float)(PWM_MAX/max);
        }
    }
}

int Motor_PI_FL(int Encoder_Value, int Target)
{
    static int Bias0, Last_Bias0, PWM0;
    Bias0 = Encoder_Value - Target;
    PWM0 -= Kp[0]*(Bias0-Last_Bias0)+Ki[0]*Bias0;
    Last_Bias0 = Bias0;
    return PWM0;
}

int Motor_PI_FR(int Encoder_Value, int Target)
{
    static int Bias1, Last_Bias1, PWM1;
    Bias1 = Encoder_Value - Target;
    PWM1 -= Kp[1]*(Bias1-Last_Bias1)+Ki[1]*Bias1;
    Last_Bias1 = Bias1;
    return PWM1;
}

int Motor_PI_BL(int Encoder_Value, int Target)
{
    static int Bias2, Last_Bias2, PWM2;
    Bias2 = Encoder_Value - Target;
    PWM2 -= Kp[2]*(Bias2-Last_Bias2)+Ki[2]*Bias2;
    Last_Bias2 = Bias2;
    return PWM2;
}

int Motor_PI_BR(int Encoder_Value, int Target)
{
    static int Bias3, Last_Bias3, PWM3;
    Bias3 = Encoder_Value - Target;
    PWM3 -= Kp[3]*(Bias3-Last_Bias3)+Ki[3]*Bias3;
    Last_Bias3 = Bias3;
    return PWM3;
}
